#include "UWBManager.h"
#include <algorithm>
#include <fcntl.h>
#include <iostream>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <unistd.h>

#define NODE_HEADER_LEN_FRAME0 11
#define NODE_HEADER_LEN_FRAME1 27
#define NODE_HEADER_LEN_FRAME2 119
#define NODE_HEADER_LEN_FRAME3 21
#define REMOTE_HEADER_LENGTH0 4

int HEADER_LENGTH[4] = {NODE_HEADER_LEN_FRAME0,
                        NODE_HEADER_LEN_FRAME1,
                        NODE_HEADER_LEN_FRAME2,
                        NODE_HEADER_LEN_FRAME3};
/**
 * @description: 初始化uwb模块
 * @param 串口设备名，波特率
 */
UWBManager::UWBManager(std::string serial_name,
                             int baudrate,
                             bool enable_debug_output){
                                 setUWBManager(serial_name,baudrate,enable_debug_output);
                             }


void
UWBManager::setUWBManager(std::string serial_name,
                             int baudrate,
                             bool enable_debug_output) {
    this->enable_debug_output = enable_debug_output;
    while (!openPort(serial_name, baudrate)){
        printf("[ERROR] Can't open serial port!!!!CHECK YOUR SETTING retrying ......;\n");
        usleep(1000000);
        uwb_ok = false;
    }
    printf("Open port %s[%d] successful!\n\n", serial_name.c_str(), baudrate);
    uwb_ok = true;
}

bool 
UWBManager::openPort(std::string serial_name, int baudrate) {
    printf("Trying to open port %s\n", serial_name.c_str());
    serial_fd = open(serial_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (serial_fd == -1) {
        perror("open_port: Unable to open port ");
        return false;
    } else
        fcntl(serial_fd, F_SETFL, 0);
    configurePort(baudrate);
    return true;
}

void 
UWBManager::configurePort(int baudrate) {
    struct termios tty;
    if (tcgetattr(serial_fd, &tty) < 0) {
        printf("Error from tcgetattr");
        exit(-1);
        return;
    }
    speed_t spd = B921600;
    switch (baudrate) {
        case 3000000:
            spd = B3000000;
            break;
        case 1000000:
            spd = B1000000;
            break;
        case 230400:
            spd = B230400;
            break;
        case 460800:
            spd = B460800;
            break;
        case 921600:
            spd = B921600;
            break;
        case 115200:
        default:
            spd = B115200;
            break;
    }
    cfsetospeed(&tty, spd);
    cfsetispeed(&tty, spd);

    tty.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= CS8;      /* 8-bit characters */
    tty.c_cflag &= ~PARENB;  /* no parity bit */
    tty.c_cflag &= ~CSTOPB;  /* only need 1 stop bit */
    tty.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */

    /* setup for non-canonical mode */
    tty.c_iflag &=
            ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
    tty.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
    tty.c_oflag &= ~OPOST;

    /* fetch bytes as they become available */
    tty.c_cc[VMIN] = 1;
    tty.c_cc[VTIME] = 1;

    if (tcsetattr(serial_fd, TCSANOW, &tty) != 0) {
        printf("Error from tcsetattr");
        exit(-1);
        return;
    }
    printf("Successful set port\n");
    return;
}

void 
UWBManager::deleteFirstNBytesInBuf(int _len) {
    for (size_t i = 0; i < _len; i ++) {
        sum_check = (sum_check + buf[i])%256;
    }
    buf.erase(buf.begin(), buf.begin() + _len);
}

void
 UWBManager::reset_checksum() {
    sum_check = 0;
}

void 
UWBManager::readAndParse() {
    while (serialAvailableBytes() > 0) {
        uint8_t c = readBytesFromSerial();
        buf.push_back(c);
        auto frame_type = this->parseData();
        if (frame_type == NODE_DISTANCE_FRAME) {
            if(enable_debug_output){
                std::cout << "update distance node frame" <<std::endl;
            }
            this->onNodeDataUpdated();
        }
        if (frame_type == NODE_LITTLE_DISTANCE_FRAME) {
            if(enable_debug_output){
                std::cout << "update little distance node frame" <<std::endl;
            }
            this->onNodeDataUpdated();
        }        
    }
}

int 
UWBManager::parseData() {
    if (this->read_status == WAIT_FOR_HEADER) {
        this->parseHeader();
    }

    else if (this->read_status == WAIT_FOR_NODE_DETAIL) {
        bool ret = false;
        if (this->read_wait_remote_node_num > 0) {
            if (recv_type_now == NODE_BROAD_CAST_FRAME) {
                ret = parseRemoteNodeBroadCastFrame();
            } else if (recv_type_now == NODE_DISTANCE_FRAME) {
                ret = parseRemoteNodeDistanceFrame();
            } else if (recv_type_now == NODE_LITTLE_DISTANCE_FRAME){
                ret = parseRemoteNodeLittleDistanceFrame();
            }
            if (ret) {
                this->read_wait_remote_node_num--;
            }
        } else {
            this->read_status = WAIT_FOR_NODE_CHECKSUM;
        }
    }

    else if (this->read_status == WAIT_FOR_NODE_CHECKSUM) {
        if (buf.size() > 0) {
            uint8_t checksum = buf[0];
            uint8_t _checksum = sum_check;
            this->deleteFirstNBytesInBuf(1);
            this->read_status = WAIT_FOR_HEADER;
            // printf("parse_data sum_check %d checksum %d\n", _checksum, checksum);
            if (_checksum != checksum) {
                printf("parse_data CHECKSUM ERROR sum_check %d checksum %d\n", (int)sum_check, (int)checksum);
                return -1;
            }
            return this->recv_type_now;
        }
    }
    return -1;
}

void 
UWBManager::sendBroadcastData(std::vector<uint8_t> msg) {
    this->serialWrite((uint8_t *) msg.data(), msg.size());
}

void 
UWBManager::onNodeDataUpdated() {
    for (auto it : nodes_info) {
        int _id = it.first;
        if (_id < MAX_NUM) {
            if (active_node_set.find(_id) != active_node_set.end())
                nodes_info[_id].active = true;
            else
                nodes_info[_id].active = false;
        }
    }

    if (this->enable_debug_output) {
        for (auto it : nodes_info) {
            std::string str = it.second.to_str();
            printf("%s;;", str.c_str());
        }
    }

    valid_node_quantity = active_node_set.size();
    this->active_node_set.clear();
}

int 
UWBManager::serialAvailableBytes() {
    int bytes = 0;
    ioctl(serial_fd, FIONREAD, &bytes);
    return bytes;
}

uint8_t
UWBManager::readBytesFromSerial() {
    char c;
    if (read(serial_fd, &c, sizeof(char))) {};
    return c;
}


int UWBManager::parseValidNodeQuantityDistanceFrame() {
    NodeValidNodeQuantityDistanceFrame nh{};
    memcpy(&nh, buf.data(), sizeof(nh));
    this->sys_time = nh.lps_time;
    this->self_id = nh.id;
    this->valid_node_quantity = nh.valid_node_quantity;
    this->deleteFirstNBytesInBuf(NODE_HEADER_LEN_FRAME2);
    return this->valid_node_quantity;
}

int UWBManager::parseValidNodeQuantityLittleDistanceFrame(){
    NodeValidNodeQuantityLittleDistanceFrame nh{};
    memcpy(&nh, buf.data(), sizeof(nh));
    this->sys_time = nh.lps_time;
    this->self_id = nh.id;
    this->valid_node_quantity = nh.valid_node_quantity;
    this->deleteFirstNBytesInBuf(NODE_HEADER_LEN_FRAME3);
    return this->valid_node_quantity;
}

int UWBManager::parseRecvNodeNumsFrame() {
    NodeRecvNodeNumsFrame nh{};
    memcpy(&nh, buf.data(), sizeof(nh));
    this->self_id = nh.id;
    this->deleteFirstNBytesInBuf(NODE_HEADER_LEN_FRAME0);
    return nh.recv_node_nums;
}

bool UWBManager::isNodeMsgHeader() {
    return (buf[0] == 0x55 && buf[1] == 0x02) ||
           (buf[0] == 0x55 && buf[1] == 0x03) ||
           (buf[0] == 0x55 && buf[1] == 0x04) ||
           (buf[0] == 0x55 && buf[1] == 0x05);
}

uint8_t
UWBManager::frameType() {
    if (buf[0] == 0x55 && buf[1] == 0x02) {
        return NODE_BROAD_CAST_FRAME;
    }

    if (buf[0] == 0x55 && buf[1] == 0x03) {
        return NODE_FRAME1;
    }

    if (buf[0] == 0x55 && buf[1] == 0x04) {
        return NODE_DISTANCE_FRAME;
    }
    
    if (buf[0] == 0x55 && buf[1] == 0x05) {
        return NODE_LITTLE_DISTANCE_FRAME;
    }

    return 255;
}

void UWBManager::parseHeader() {
    while (buf.size() > 2 && !isNodeMsgHeader()) {
        deleteFirstNBytesInBuf(1);
    }
    reset_checksum();
    recv_type_now = frameType();
    if (recv_type_now == 255) {
        return;
    }
    size_t recv_nums = HEADER_LENGTH[recv_type_now];
    if (buf.size() < recv_nums) {
        return;
    }

    int node_num = 0;
    switch (recv_type_now) {
        case NODE_BROAD_CAST_FRAME:
            node_num = this->parseRecvNodeNumsFrame();

            if (node_num > MAX_NUM || node_num < 0) {
                printf("Error Node num; throw");
                return;
            }
            if(enable_debug_output)
                printf("Msg recv, node num %d\n", node_num);

            break;
        case NODE_FRAME1:
            fprintf(stderr, "FRAME1 detected! Exit");
            // exit(-1);
            break;
        case NODE_DISTANCE_FRAME:
            node_num = this->parseValidNodeQuantityDistanceFrame();
            if (sys_time > 0) {
                onSystemTimeUpdate();
            }
            if (node_num > MAX_NUM || node_num < 0) {
                printf("Error Node num; throw");
                return;
            }
            break;
        case NODE_LITTLE_DISTANCE_FRAME:
            node_num = this->parseValidNodeQuantityLittleDistanceFrame();
            if (sys_time > 0) {
                onSystemTimeUpdate();
            }
            if (node_num > MAX_NUM || node_num < 0) {
                printf("Error Node num; throw");
                return;
            }
        default:
            break;
    }

    if (node_num > MAX_NUM) {
        fprintf(stderr, "INVAILD node num %d, set to zero", node_num);
        node_num = 0;
    }

    this->read_wait_remote_node_num = node_num;
    this->read_status = WAIT_FOR_NODE_DETAIL;
}


bool UWBManager::parseRemoteNodeDistanceFrame() {
    size_t header_length = sizeof(RemoteNodeDistanceFrame);

    if (buf.size() < header_length)
        return false;
    RemoteNodeDistanceFrame nh{};
    memcpy(&nh, buf.data(), sizeof(nh));

    deleteFirstNBytesInBuf(header_length);
    this->onNodeDataRecv(nh);
    return true;
}

bool UWBManager::parseRemoteNodeLittleDistanceFrame() {
    size_t header_length = sizeof(RemoteNodeLittleDistanceFrame);

    if (buf.size() < header_length)
        return false;
    RemoteNodeLittleDistanceFrame nh{};
    memcpy(&nh, buf.data(), sizeof(nh));

    deleteFirstNBytesInBuf(header_length);
    this->onNodeDataRecv(nh);
    return true;
}

bool UWBManager::parseRemoteNodeBroadCastFrame() {
    size_t header_length = sizeof(RemoteNodeBroadCastHeader);
    if (buf.size() < header_length)
        return false;
    RemoteNodeBroadCastHeader nh{};
    memcpy(&nh, buf.data(), sizeof(nh));

    if (nh.data_length == 0) {
        if (enable_debug_output)
            printf("Zero datalength %d", nh.id);
        return true;
    } else {
        if (enable_debug_output)
            printf("recv broad cast data length D %d", nh.data_length);
        if (buf.size() < u_int(REMOTE_HEADER_LENGTH0 + nh.data_length)) {
            return false;
        } else {
            Buffer msg(nh.data_length);
            std::copy(buf.begin() + REMOTE_HEADER_LENGTH0,
                      buf.begin() + REMOTE_HEADER_LENGTH0 + nh.data_length,
                      msg.begin());

            deleteFirstNBytesInBuf(REMOTE_HEADER_LENGTH0 + nh.data_length);
            onBroadcastDataRecv(nh.id, msg);
            return true;
        }
    }
    return false;
}

void UWBManager::onBroadcastDataRecv(int _id, Buffer _msg) {
    char *str = (char *) _msg.data();
    if (this->enable_debug_output) {
        printf("RECV DATA @ %d on systime %d, msg\n%s\n",
               _id,
               this->sys_time,
               str);
    }
}

void UWBManager::serialWrite(uint8_t *data, int len) {
    if (write(serial_fd, data, len) == 0 && enable_debug_output) {
        printf("write wrong");
    };
}

void UWBManager::onNodeDataRecv(RemoteNodeDistanceFrame nf) {
    this->active_node_set.insert(nf.id);
    int _id = nf.id;
    if (enable_debug_output) {
        printf("Distance frame Insert to active node set%d\n", _id);
    }
    if (nodes_info.find(_id) == nodes_info.end()) {
        RemoteNodeInfo info;
        nodes_info[_id] = info;
        nodes_info[_id].id = _id;
    }
    nodes_info[_id].distance = ((double) nf.distance) / 1000;
    nodes_info[_id].fp_rssi = (double) nf.fp_rssi / -2.0;
    nodes_info[_id].active = true;
    nodes_info[_id].dis_time = nf.rx_lps_systime;
    if (enable_debug_output) {
        printf("ID %d RX %d\n", _id, nf.rx_lps_systime);
    }
    nodes_info[_id].rx_rssi = nf.rx_rssi / -2.0;
    nodes_info[_id].role = nf.role;
}

void UWBManager::onNodeDataRecv(RemoteNodeLittleDistanceFrame nf) {
    this->active_node_set.insert(nf.id);
    int _id = nf.id;
    if (enable_debug_output) {
        printf("Little Distance frame Insert to active node set%d\n", _id);
    }
    if (nodes_info.find(_id) == nodes_info.end()) {
        RemoteNodeInfo info;
        nodes_info[_id] = info;
        nodes_info[_id].id = _id;
    }
    nodes_info[_id].distance = ((double) nf.distance) / 1000;
    nodes_info[_id].fp_rssi = (double) nf.fp_rssi / -2.0;
    nodes_info[_id].active = true;
    nodes_info[_id].rx_rssi = nf.rx_rssi / -2.0;
    nodes_info[_id].role = nf.role;
}
